/**
 * @file   MiraAffineTrans2D.cpp
 * @author Josep Sabater (jsabaterm@el.ub.edu)
 * @date   September, 2015
 * @version 1.0.0
 * @ingroup MiraGeometryLib
 * @brief  Implementation of Affine Transformations in 2 dimensions.
 *
 *
 */

#include <cmath>
#include <cstdio>
#include <iostream>
#include <algorithm>

#include "MiraAffineTrans2D.h"
#include "MiraMath.h"


namespace MiraGeometryLib{
    AffineTrans2D::AffineTrans2D(const double& dX,
                                 const double& dY,
                                 const double& r,
                                 const double& hw){
        m00_ = cos(r);
        m01_ = -sin(r);
        m10_ = sin(r);
        m11_ = cos(r);

        m02_ = dX;
        m12_ = dY;
        m22_ = hw;
    }

    AffineTrans2D:: AffineTrans2D(const double& m00,
                                  const double& m01,
                                  const double& m02,
                                  const double& m10,
                                  const double& m11,
                                  const double& m12,
                                  const double& m22):
       m00_(m00), m01_(m01), m02_(m02),
       m10_(m10), m11_(m11), m12_(m12),
       m22_(m22) {
    }

   Point2DHom AffineTrans2D::transform(const Point2DHom& ph) const {
        double x = ph.x();
        double y = ph.y();
        double h = ph.h();

        return Point2DHom(m00_*x + m01_*y + m02_*h,
                          m10_*x + m11_*y + m12_*h,
                          m22_*h);
    }

    AffineTrans2D AffineTrans2D::invert() const{
        return AffineTrans2D(m11_*m22_,
                             -m01_*m22_,
                             m01_*m12_ - m11_*m02_,
                             -m10_*m22_,
                             m00_*m22_,
                             m10_*m02_ -  m00_*m12_,
                             m00_*m11_ - m01_*m10_);
    }

    AffineTrans2D AffineTrans2D::flip() const{
        return AffineTrans2D(-m00_, m01_, m02_, -m10_, m11_, m12_, m22_);
    }

    AffineTrans2D AffineTrans2D::operator * (const AffineTrans2D& r) const {

        return AffineTrans2D(m00_*r.m00_ + m01_*r.m10_,
                             m00_*r.m01_ + m01_*r.m11_,
                             m00_*r.m02_ + m01_*r.m12_ + m02_*r.m22_,
                             m10_*r.m00_ + m11_*r.m10_,
                             m10_*r.m01_ + m11_*r.m11_,
                             m10_*r.m02_ + m11_*r.m12_ + m12_*r.m22_,
                             m22_*r.m22_);
    }

    AffineTrans2D& AffineTrans2D::operator *= (const AffineTrans2D& r){
        m00_ = m00_*r.m00_ + m01_*r.m10_;
        m01_ = m00_*r.m01_ + m01_*r.m11_;
        m02_ = m00_*r.m02_ + m01_*r.m12_ + m02_*r.m22_;

        m10_ = m10_*r.m00_ + m11_*r.m10_;
        m11_ = m10_*r.m01_ + m11_*r.m11_;
        m12_ = m10_*r.m02_ + m11_*r.m12_ + m12_*r.m22_;
        m22_ = m22_*r.m22_;

        return *this;
    }
    
    /** Non-member friend function for << operator
     */
    std::ostream& operator << (std::ostream& os, const AffineTrans2D& aff){
        static const char *fmt = "\t%f %f %f\n \t%f %f %f\n \t%f %f %f";
        int sz = std::snprintf(nullptr, 0, fmt,
                      aff.m00_,
                      aff.m01_,
                      aff.m02_,
                      aff.m10_,
                      aff.m11_,
                      aff.m12_,
                      0.0,
                      0.0,
                      aff.m22_);
        std::vector<char> buf(sz + 1); // note +1 for null terminator
       
        std::snprintf(&buf[0], buf.size(), fmt,
                      aff.m00_,
                      aff.m01_,
                      aff.m02_,
                      aff.m10_,
                      aff.m11_,
                      aff.m12_,
                      0.0,
                      0.0,
                      aff.m22_);

        std::string s(buf.begin(), buf.end());        
        return os << s;
    }
}
